#pragma once
#include "opencv2/core/core.hpp"
#include<iostream>
#include <opencv2/opencv.hpp>
#include"../General/General.h"
//#define DEBUG

namespace rm
{
double gravityOffset(double v, double theta, double _euclideanDistance);
struct AngleSolverParam
{
    cv::Mat CAM_MATRIX;			// 摄像头内参矩阵
    cv::Mat DISTORTION_COEFF;	// 摄像头畸变参数
    //装甲板等目标的实际尺寸
    static std::vector<cv::Point3f> POINT_3D_OF_ARMOR_BIG;
    static std::vector<cv::Point3f> POINT_3D_OF_ARMOR_SMALL;
    static std::vector<cv::Point3f> POINT_3D_OF_RUNE;
    double Y_DISTANCE_BETWEEN_GUN_AND_CAM = 0;//If gun is under the cam, this variable is positive.
    cv::Size CAM_SIZE = cv::Size(1920, 1080);
    double GUARD_HIGHT = 4000;

    //从文件中读取参数,记得更改路径
    void readFile(const int id);
};

struct KF_Param
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
};//Kalman Filter parameter

/**
*	solve by PNP, that is, using four points to detect the angle and distance.
*	It's not very useful if the armor is far.If it' far try solve by one point
*/
class AngleSolver
{
public:
    //Predictor用
    float history_ErrAngle;
    float history_PreAngle;
    float history_PrePreAngle;
    float history_PrePrePreAngle;
    const cv::Vec2f predict(cv::Vec2f now_ErrAngle,double timeStamp);
    void initPredictor();

    AngleSolver();
    AngleSolver(const AngleSolverParam& AngleSolverParam);

    /*
     *  Initialize with parameters
     */
    void init(const AngleSolverParam& AngleSolverParam);

    enum AngleFlag
    {
        ANGLE_ERROR = 0,                //错误
        ONLY_ANGLES = 1,				//返回角度
        TOO_FAR = 2,					//距离过远
        ANGLES_AND_DISTANCE = 3			//返回角度与距离
    };


    //设置目标
    void setTarget(const std::vector<cv::Point2f> objectPoints, int objectType);
    void setTarget(const cv::Point2f centerPoint, int objectType);
    void setTarget(const std::vector<cv::Point2f> objectPoints,const cv::Point2f Center_of_armor, int objectType);


    //角度解算
    AngleFlag solve();

    /*
    *      z: direction of the shooter
    *     /
    *  O /______x
    *    |
    *    |
    *    y
    */

    //祖传代码了,应该是防止标定时使用的图片尺寸和实际不符，但可能导致错误，所以做了一些调整
    void setResolution(const cv::Size2i& image_resolution);

    //字面意思，没什么用
    void setUserType(int usertype);
    void setEnemyType(int enemytype);
    void setBulletSpeed(int bulletSpeed);

    //获取偏向角,距离等
    const cv::Vec2f getAngle();
    const cv::Point3f getDirection();
    double getDistance();
    double KF_Distance();   //对距离使用卡尔曼滤波
    void KF_Refresh();


#ifdef DEBUG

    void showPoints2dOfArmor();
    void showTvec();    
    void showEDistance();    
    void showcenter_of_armor();    
    void showAngle();   
    int showAlgorithm();
#endif // DEBUG

private:
    AngleSolverParam _params;
    KF_Param KFP_distance = {0.02, 0, 0, 0, 0.001, 0.543};

    cv::Mat _rVec = cv::Mat::zeros(3, 1, CV_64FC1);//PnP解算输出的旋转向量
    cv::Mat _tVec = cv::Mat::zeros(3, 1, CV_64FC1);//PnP解算输出的平移矩阵
    std::vector<cv::Point2f> point_2d_of_armor;//PnP解算的2D部分,目标在相机中的点
    std::vector<cv::Point2f> point_2d_of_rune;
    enum solverAlg
    {
        ONE_POINT = 0,
        PNP4 = 1
    };
    int angle_solver_algorithm = 1;//if 1 ,using PNP solution, if 0 using OnePoint solution,if 2 solving the rune by pnp.

    cv::Point2f centerPoint;
    std::vector<cv::Point2f> target_nothing;

    cv::Point3f target_direction;
    double _xErr = 0, _yErr = 0, _euclideanDistance = 0;
    double pre_euclideanDistance = 0;
    double pre_xErr = 0;
    double pre_yErr = 0;

    cv::Size2i image_size = cv::Size2i(1280, 1024);
    int user_type = 1;
    int enemy_type = 1;
    double _bullet_speed = 22000;
    double _rune_compensated_angle = 0;
    int is_shooting_rune = 0;
    cv::Mat _cam_instant_matrix;
};

}
